
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.robotics.AnalogInput;
import javax.comm.SerialPort;

public class SlaveBoard {

    private Mailbox mailbox;
    protected AnalogShaftEncoder rightEncoder;
    protected AnalogShaftEncoder leftEncoder;
    protected AnalogInput rightWheelInput = IntelliBrain.getAnalogInput(5);
    protected AnalogInput leftWheelInput = IntelliBrain.getAnalogInput(4);
    protected ReceiveThread recThread;

    public SlaveBoard() {
        //Create the shaft encoders for both motors
        rightEncoder = new AnalogShaftEncoder(
                rightWheelInput, 250, 750, 30,
                Thread.MAX_PRIORITY);

        leftEncoder = new AnalogShaftEncoder(
                leftWheelInput, 250, 750, 30,
                Thread.MAX_PRIORITY);
        connectToMaster();
        recThread = new ReceiveThread(mailbox);
        recThread.start();
        go();
    }

    public void go() {
        while (true) {
            try {
                mailbox.postMessage(new byte[]{0, (byte) leftEncoder.getCounts(), (byte) rightEncoder.getCounts()});
                byte[] msg = recThread.getMessage();
                if (msg != null) {
                    if (msg[0] == 0) {
                    leftEncoder.resetCounts();
                    rightEncoder.resetCounts();
                }
                }
            } catch (Exception ex) {
            }
        }
    }

    private void connectToMaster() {
        try {
            SerialPort port = IntelliBrain.getCom1();
            port.setSerialPortParams(115200, SerialPort.DATABITS_8, SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
            mailbox =
                    new Mailbox(port.getInputStream(), port.getOutputStream(), 50, 1);
            mailbox.open();
        } catch (Exception ex) {
        }
    }

    public static void main(String[] args) {
        new SlaveBoard();
    }
}
